#!/usr/bin/env python
# -*- coding: utf-8 -*-

import envs
import rospy
import actionlib

from npu_robot.msg import RobotTrackAction, RobotTrackGoal


class RobotObject():
    def __init__(self):
        # 小车跟随垃圾客户端
        self.robot_track_client = actionlib.SimpleActionClient('/robot_track', RobotTrackAction)

        # 小车跟随垃圾
        self.goal = RobotTrackGoal()

    def active_callback(self):
        """
        action激活后回调（一次）
        """
        rospy.logdebug('robot track action is actived')

    def done_callback(self, state, res):
        """
        action完成后回调（一次）
        """
        rospy.logdebug('robot track action is finished')
        rospy.logdebug('state: %s', state)
        rospy.logdebug('res: %s', res)

    def feedback_callback(self, feedback):
        """
        action执行过程中反馈的回调（多次）
        """
        rospy.logdebug('robot track action is feedbacking...')
        rospy.logdebug('feedback: %s', feedback)

    def robot_track_send_goal(self, goal):
        """
        action请求
        """
        self.robot_track_client.wait_for_server()

        self.robot_track_client.send_goal(goal, done_cb=self.done_callback, active_cb=self.active_callback, feedback_cb=self.feedback_callback)
        self.robot_track_client.wait_for_result(rospy.Duration.from_sec(300.0))

    def robot_track(self):
        """
        小车跟随垃圾
        """
        rospy.loginfo('[robot_object] robot track...')

        self.goal.id = 1
        self.robot_track_send_goal(self.goal)

        rospy.loginfo('[robot_object] robot track OK')


if __name__ == '__main__':
    rospy.init_node('npu_robot_object', anonymous=True, log_level=envs.log_level)
    robot_object = RobotObject()
    robot_object.robot_track()
